#ifndef __A_STAR_H__
#define __A_STAR_H__

#include <ros/ros.h>
#include <iostream>
#include <sstream>
#include <algorithm>
#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>
#include <Eigen/Dense>

#include "../common/grid_point.h"
using namespace std;
using namespace cv;
using namespace Eigen; 

class AstarPlanner{
    public:
        AstarPlanner();
        ~AstarPlanner();

        // 设置调试模式
        void SetDebugMode(bool mode);
        // 设置地图
        void SetMap(cv::Mat map);
        // 计算到起点代价
        float CalCostG(cv::Point point, cv::Point start, float D);
        // 计算到终点代价
        float CalCostH(cv::Point point, cv::Point goal, float D);
        // 计算点是否有效
        bool IsValid(cv::Point point);
        // 判断两点是否相等
        bool IsEqual(cv::Point point1, cv::Point point2);
        // 规划路径
        vector<cv::Point> GetPath(cv::Point start, cv::Point goal, vector<cv::Point>& expand_poinds);

    private:
        bool __debug_mode;      // 调试模式
        cv::Mat __map_mat;      // 地图数据
        vector<pair<cv::Point, float>> __neighbor_points;

        Eigen::MatrixXf __cost_cache_mat;
};

#endif